/**
 * @file Dog.h
 * @brief
 * @author Luke (AiNuozhou@bupt.edu.cn)
 * @version 1.0
 * @date 2022-02-05
 *
 * @copyright Copyright (c) 2022  北京邮电大学
 *
 * @par 修改日志:
 * @date 2022-02-05
 * @author Luke (AiNuozhou@bupt.edu.cn)
 */

#ifndef DOG_H
#define DOG_H

#include "oslib.h"
#include "servo.h"
#include "utils.h"

#define LEG_LA 0.079
#define LEG_LB 0.215
#define LEG_LC 0.03
#define LEG_L1 0.0427
#define LEG_L2 0.0307
#define LEG_L3 0.0738
#define LEG_L4 0.09

//伸直状态足端坐标
#define LEG0_RESET_POS_X 0.0822
#define LEG0_RESET_POS_Y -0.1795
#define LEG0_RESET_POS_Z 0.1075

#define LEG1_RESET_POS_X -0.0822
#define LEG1_RESET_POS_Y -0.1795
#define LEG1_RESET_POS_Z 0.1075

#define LEG2_RESET_POS_X -0.0822
#define LEG2_RESET_POS_Y -0.1795
#define LEG2_RESET_POS_Z -0.1075

#define LEG3_RESET_POS_X 0.0822
#define LEG3_RESET_POS_Y -0.1795
#define LEG3_RESET_POS_Z -0.1075

//启动状态足端坐标
#define LEG_INIT_DELTA_X 0;
#define LEG_INIT_DELTA_Y 0.03;
#define LEG_INIT_DELTA_Z 0;

#define LEG0_INIT_POS_X LEG0_RESET_POS_X + LEG_INIT_DELTA_X
#define LEG0_INIT_POS_Y LEG0_RESET_POS_Y + LEG_INIT_DELTA_Y
#define LEG0_INIT_POS_Z LEG0_RESET_POS_Z + LEG_INIT_DELTA_Z

#define LEG1_INIT_POS_X LEG1_RESET_POS_X + LEG_INIT_DELTA_X
#define LEG1_INIT_POS_Y LEG1_RESET_POS_Y + LEG_INIT_DELTA_Y
#define LEG1_INIT_POS_Z LEG1_RESET_POS_Z + LEG_INIT_DELTA_Z

#define LEG2_INIT_POS_X LEG2_RESET_POS_X + LEG_INIT_DELTA_X
#define LEG2_INIT_POS_Y LEG2_RESET_POS_Y + LEG_INIT_DELTA_Y
#define LEG2_INIT_POS_Z LEG2_RESET_POS_Z + LEG_INIT_DELTA_Z

#define LEG3_INIT_POS_X LEG3_RESET_POS_X + LEG_INIT_DELTA_X
#define LEG3_INIT_POS_Y LEG3_RESET_POS_Y + LEG_INIT_DELTA_Y
#define LEG3_INIT_POS_Z LEG3_RESET_POS_Z + LEG_INIT_DELTA_Z

// #define GAIT_S 0.035 //前进距离
// #define GAIT_H 0.035 //抬腿高度
// #define GAIT_T 2     //每个步态的时间周期
#define TAIJIE_PARM 0.05

typedef enum
{
  DOG_MODE_NONE = 0,
  DOG_MODE_STOP,
  DOG_MODE_RESET,
  DOG_MODE_ADJUST,
  DOG_MODE_BALANCE,
  DOG_MODE_WALK,      // 前进
  DOG_MODE_WALK_OMNI, // 全向
  DOG_MODE_PACE,
  DOG_MODE_PACE_OMNI,
  DOG_MODE_UPSTAIR,
  DOG_MODE_DEBUG,
  DOG_MODE_DOWNSTAIR,
  DOG_MODE_TEST

} DogMode_e;

typedef struct
{
  uint8_t No;
  Servo_s servo[3];
  double foot_init_x;
  double foot_init_y;
  double foot_init_z;
  double foot_target_x;
  double foot_target_y;
  double foot_target_z;
  uint8_t configuration;
} Leg_s;

typedef struct
{
  float v_x;
  float v_y;
  float v_z;

  float speed;

  float now_x;
  float now_y;
  float now_z;

  float now_acc_x;
  float now_acc_y;
  float now_acc_z;

  float target_x;
  float target_y;
  float target_z;

  float target_yaw;
  float target_pitch;
  float target_roll;

  float now_yaw;
  float now_pitch;
  float now_roll;

  float now_RollSpeed;
  float now_PitchSpeed;
  float now_YawSpeed;

  uint8_t pose_mode;

  uint8_t phase[4];

  Leg_s legs[4];
  DogMode_e ctrl_mode;
} Dog_s;

extern Dog_s Dog;
extern PID_s RollPID;
extern PID_s Pitch_PID;

extern float delta_h;
extern float delta_z;
extern float delta_t;
extern float delta_x;
extern float delta_dh[4];

extern float GAIT_S;
extern float GAIT_H;
extern float GAIT_T; // 步态周期
extern float GAIT_DZ;

extern float gait_delta_x;
extern float gait_delta_y;
extern float gait_delta_z;

extern float delta_z_front;
extern float delta_z_back;

extern float turn_angle;

extern float barycenter_front;
extern float barycenter_back;
extern float barycenter_left;
extern float barycenter_right;
extern float barycenter_central;

extern float phase_graph[4][2];

extern int debug_leg;
extern float debug_target_x;
extern float debug_target_y;
extern float debug_target_z;

extern uint8_t speed_mode;

extern uint8_t climb_first_flag;
extern Time_s Dog_time;


void Dog_init();
void Dog_exe();
void Dog_Inverse_kinematics(Leg_s *leg);
void Dog_Gait_Reset();
void Dog_Gait_Reinit();
void Dog_Gait_Turn();
void Dog_Balance();
void Dog_Pose_Adjust();
void Dog_Gait_Turn_in_place();
void Dog_servo_angle_pos();
void Dog_servo_pos_angle();
void Dog_Gait_Walk();
void Dog_Gait_Walk_Omni();
void Dog_Gait_Pace();
void Dog_Gait_Pace_Omni();
void Dog_Gait_shangtaijie_zuoyou();
void Dog_Gait_Upstair();
void Dog_Gait_swing();
void Dog_Leg_Point_test(int leg, float target_x, float target_y, float target_z);
void Dog_Gait_DownStair();
void Dog_Gait_Test();

#endif // DOG_H